package dijkstra;

import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Set;

import be.kuleuven.cs.som.annotate.Basic;
import be.kuleuven.cs.som.annotate.Immutable;

import roborally.factory.PhysicalAmountFactory;
import roborally.itemsonboard.Robot;
import roborally.supporting.DefaultConstants;
import roborally.supporting.PhysicalAmount;
import roborally.supporting.Unit;
import roborally.supporting.Orientation;

/**
 * This class implements the shortest path Dijkstra algorithm. The specific implementation is
 * founded in subclasses. This class has only the needed equipment and support for used as a thread.
 * 
 * The class focus on calculating the shortest path as fast as possible but allows some
 * redundant information to store the temporal results. There is no implementation given,
 * only the containers and the help methods.
 * The class implements the Runnable interface so that multiple threads are supported.
 * 
 * @author Molderez Tom, 2de bach Ingenieurswetenschappen Computerwetenschappen-Elektrotechniek and Reniers Jorn, 2de bach Ingenieurswetenschappen Werktuigkunde-Computerwetenschappen
 * @version 2.0
 *
 */ 
public abstract class Dijkstra implements Runnable {
			
	
	/**
	 * Creates a new Dijkstra object to calculate the most efficient path.
	 * 
	 * @param robot The robot that has to move.
	 * @pre The robot must be valid.
	 * 		| isValidRobot(robot)
	 */
	protected Dijkstra(Robot robot){
		assert isValidRobot(robot);
		this.robot = robot;
	}
	
	/**
	 * Returns the visited vertexes. 
	 * 
	 * @note Be careful, a reference will be returned.
	 */
	@Basic
	protected Set<Vertex> getVisited() {
		return visited;
	}
	
	/**
	 * A set containing the visited vertexes. The minimum distance from the robot to vertexes in this set are known.
	 */
	private final Set<Vertex> visited = new HashSet<Vertex>();
	
	/**
	 * Returns the current path, existing of a map with as key a Vertex and as value the vertex from which this key is reached.
	 * This path is the path that consumes the least energy to reach all positions that are in the path starting from the position
	 * of the robot.
	 * @note The path is returned in the reverse order.
	 * @note Be careful, a reference will be returned.
	 */
	@Basic
	protected Map<Vertex,Vertex> getCurrentPath() {
		return currentPath;
	}
	
	/**
	 * A map containing the currentPath the robot must follow to reach the minimum position in reverse order.
	 */
	private final Map<Vertex,Vertex> currentPath = new HashMap<Vertex,Vertex>();
	
	/**
	 * Returns a map containing as key a Vertex and as value the energy cost to reach this Vertex from the starting position of the robot.
	 * @note Be careful, a reference will be returned.
	 */
	@Basic
	protected Map<Vertex, PhysicalAmount> getDistances() {
		return distances;
	}
	/**
	 * A map containing the distance (expressed in energyCost) to reach a given vertex. We use a map so we can ask the distance in 
	 * nearly constant time. (We could also use the priorityQueue for that purpose but the time performance is weak)
	 */
	private final Map<Vertex,PhysicalAmount> distances = new HashMap<Vertex, PhysicalAmount>();
	
	/**
	 * Returns the unvisited Vertexes sorted by their distance from the start vertex.
	 * @note Be careful, a reference will be returned.
	 */
	@Basic
	protected PriorityQueue<Vertex> getUnvisited() {
		return unvisited;
	}
	
	/**
	 * A priorityQueue containing the unvisited vertexes, sorted by their distance from the start vertex.
	 */
	private final PriorityQueue<Vertex> unvisited = new PriorityQueue<Vertex>();
	
	/**
	 * Checks if the robot is valid.
	 * 
	 * @param robot The robot to be controlled.
	 * @return The robot must be valid and must have a board.
	 * 		| result == !(robot == null || robot.getBoard() == null)
	 */
	public static boolean isValidRobot(Robot robot) { 
		return !(robot == null || robot.getBoard() == null); 
	}
	
	/**
	 * Returns the robot that must moved by this Dijkstra object.
	 */
	@Basic @Immutable
	public Robot getRobot() {
		return this.robot;
	}
	
	private final Robot robot;

	
	/**
	 * Returns the state of this Dijkstra object.
	 * 
	 * The state becomes true when the shortest path is calculated using the method execute. Then, this Dijkstra object can modify the robot properly.
	 */
	@Basic
	public DijkstraState getState() {
		return this.state;
	}
	
	/**
	 * Sets the state of this Dijkstra to the given state.
	 * 
	 * @param state The new state of the Dijkstra-object.
	 * @post 
	 * 		| (new this).getState() == state
	 */
	protected void setState(DijkstraState state) {
		this.state = state;
	}
	
	/**
	 * A variable representing the state of this Dijkstra-object.
	 */
	private DijkstraState state = DijkstraState.NOT_EXECUTED;
	
	/**
	 * Performs the Dijkstra algorithm. After invoking this method,
	 * the client can ask this to return the results.
	 */
	@Override
	public abstract void run();

	/**
	 * Initializes the containers with proper values and initializes the start values.
	 */
	protected void initialise() {
		Vertex initalVertex = new Vertex(this.getRobot().getPosition(), this.getRobot().getOrientation(),PhysicalAmountFactory.get().create(0, Unit.WATTSECOND));
		
		getVisited().clear();
		getCurrentPath().clear();
		getDistances().clear();
		getUnvisited().clear();
		
		getUnvisited().add(initalVertex);
		getDistances().put(initalVertex, PhysicalAmountFactory.get().create(0D,Unit.WATTSECOND));
	}
	
	/**
	 * Calculate the energy to move from a given vertex to the other. Both vertexes must be next to each other,
	 * but may not be the same.
	 * 
	 * @param vertexFrom The vertex from. 
	 * @param vertexTo The vertex to.
	 * @param neededTurns The turns needed to move to vertexTo.
	 * @pre
	 * 		| vertexFrom.nextTo(vertexTo)
	 * @pre
	 * 		| !vertexFrom.equals(vertexTo)
	 * @return
	 * 		| result == DefaultConstants.ROBOT_ENERGY_MOVEMENT.add(this.getRobot().getWeightCarried().multiply(DefaultConstants.ROBOT_ENERGY_MOVEMENT_PER_WEIGHT),
	 *		|	DefaultConstants.ROBOT_ENERGY_TURN.multiply(Math.abs(neededTurns)), vertexFrom.getEnergyToReach())
	 */
	protected PhysicalAmount calculateEnergy(Vertex vertexFrom, Vertex vertexTo, int neededTurns) {
		assert vertexFrom.nextTo(vertexTo);
		assert !vertexFrom.equals(vertexTo);
		return DefaultConstants.ROBOT_ENERGY_MOVEMENT.add(PhysicalAmountFactory.get().create(this.getRobot().getWeightCarried().getAmount() * DefaultConstants.ROBOT_ENERGY_MOVEMENT_PER_WEIGHT, Unit.WATTSECOND),
				DefaultConstants.ROBOT_ENERGY_TURN.multiply(Math.abs(neededTurns)), vertexFrom.getEnergyToReach());	
	}
	
	/**
	 * When the vertex is already visited, but a cheaper way exits, the storage containers will be modified.
	 * 
	 * More precisely, the vertex is removed from the priorityQueue. Then, the energy of the vertex is changed according the given parameter. The distanceces map is
	 * updated. Finally the vertex is re-added to the priorityQueue with the given energy.
	 * @param vertex The vertex to modify.
	 * @param energy The new energy to reach this vertex.
	 * @pre
	 * 		| energy < vertex.getEnergyToReach()
	 * @effect
	 * 		| vertex.setEnergyToReach(energy)
	 */
	protected void setCheapestPath(Vertex vertex, PhysicalAmount energy) {
		
		assert energy.compareTo(vertex.getEnergyToReach()) == -1;
		
		unvisited.remove(vertex);
		
		vertex.setEnergyToReach(energy);
		distances.put(vertex, energy);
		
		
		unvisited.add(vertex);
	}

	/**
	 * Calculates the turns needed before you can move efficiently to reach a given vertex.
	 *
	 * @param from The vertex to move from.
	 * @param destination The vertex to be reached.
	 * @pre 
	 * 		| from != null
	 * @pre
	 * 		| destination != null
	 * @pre
	 * 		| !from.equals(destination)
	 * @return If the orientation of the vertex is a good direction, the result will be zero (the robot doen't have to turn before the robot has to move to the given position)
	 * 			If the robot has to turn at least one time clockwise in order to look in a good direction, the result will be one.
	 * 			If the robot has to turn at least one time counterclockwise in order to look in a good direction, the result will be minus one.
	 * 			If the robot has to turn at least two times in order to look in a good direction, the result will be two.
	 */
	protected int turnsBeforeMovement(Vertex from, Vertex destination){
		assert from != null;
		assert destination != null;
		assert !from.equals(destination);
		
		Set<Integer> movementPossibilities = new HashSet<Integer>();
		
		if(destination.getPosition().getX()-from.getPosition().getX()>0)
			movementPossibilities.add(Orientation.RIGHT.getOrientation());
		if(destination.getPosition().getX()-from.getPosition().getX()<0)
			movementPossibilities.add(Orientation.LEFT.getOrientation());
		if(destination.getPosition().getY()-from.getPosition().getY()>0)
			movementPossibilities.add(Orientation.DOWN.getOrientation());
		if(destination.getPosition().getY()-from.getPosition().getY()<0)
			movementPossibilities.add(Orientation.UP.getOrientation());
		
		int step = 0; // the order to control: 0,1,-1,2
		boolean found = false;
		while(step < 3 && !found) {
			if(movementPossibilities.contains((from.getOrientation().getOrientation() + step +4)%4))
				found = true;
			else {
				step++;
				if(step == 2) step = -1; // After 1, then comes -1
				else if(step == 0) step = 2; // After -1 then comes 2
			}				
		}
		return step;
	}
	
	
}
